﻿using System;
using System.IO;
using System.Net.Sockets;
using System.Threading;
using ControlSystem;
using SenRobCSTest;

namespace com.atiia.automation.sensors
{
    class FTsensor
    {
        private const int refreshrate = 50;

        private int sensor_id = 0; int get_sensor_id() { return sensor_id; } // integer to distinguish between sensor 1 and 2

        private double[] m_daftMaxes = { 100, 100, 100, 100, 100, 100 }; /*maximum rated force/torque readings*/
        private double[] m_daftCountsPerUnit = { 1000000, 1000000, 1000000, 1000000, 1000000, 1000000 }; /*counts per
        *unit force or torque for each axis*/
        /** Visualizes the forces and torques. */

        private String m_strForceUnits; /*The units of force.*/
        private String m_strTorqueUnits; /*The units of torque.*/
        private String m_strCalSN;
        private String m_strCalIndex;
        private String m_strCfgName;
        private String m_strCfgIndex;

        //private JProgressBar[] m_progaFTReadingBar; /*the progress bars */
        private static int NUM_FT_AXES = 6;
        //private DecimalFormat m_dfReading;
        private NetFTSensor m_netFT; /*the Net F/T controller*/
        private NetFTReaderThread m_NetFTReaderThread = null; /*reads
     Net F/T in a loop*/
        private static Socket m_cNetFTDataSocket; /*socket used for high-speed
                                                *data collection*/
        private static bool m_bDoingHighSpeedCollection = false; /*whether or not
    *we're currently doing high speed data collection*/
        private String m_strSensorAddress; /*The network address of the sensor.*/
        /** The RDT sample rate of the sensor. */
        private int m_iRDTSampleRate = 0;

        /**Thread which communicates with Net F/T
         */
        private MainForm parent;
        static Random rand = new Random();
        static Random sign = new Random();

        // control system vars
        //UDPcommand CommUDP = new UDPcommand();
        static FT Robcmnd;

        public void stop_reading() { this.m_NetFTReaderThread.stopReading(); }

        FT robCmd; public void setRobCmd(FT robCmd) { this.robCmd = robCmd; } public FT getRobCmd() { return this.robCmd; }

        NetFTRDTPacket packet; public void setRDTPacket(NetFTRDTPacket pack) { packet = pack; } public NetFTRDTPacket getRDTPacket() { return packet; }

        class NetFTReaderThread
        {
            private Thread thisThread;

            private NetFTSensor m_netFT; /*the Net F/T controller*/
            private FTsensor m_guiParent; /*the gui that is using this
                                           *thread*/

            private bool m_bKeepGoing = true; /*controls when to end this
                                              *thread*/

            public void Start() { thisThread.Start(); }

            /**
             * Creates a new Net F/T reader thread
             * @param setNetFT The initialized NetFTSensor to communicate with.
             * @param setParent The NetFTSampleGUI that is using this thread to
             * communicate with the Net F/T.
             */

            public NetFTReaderThread(NetFTSensor setNetFT, FTsensor setParent)
            {
                m_netFT = setNetFT;
                m_guiParent = setParent;

                thisThread = new Thread(run);
                thisThread.Priority = ThreadPriority.Highest;
            }

            /**Sets a flag to stop after the current read is complete*/

            public void stopReading()
            {
                m_bKeepGoing = false;
                thisThread.Interrupt(); /*wake this thread up if it's in it's sleeping
                               *phase*/
            }

            /**Reads the Net F/T sensor in a loop*/

            public void run()
            {
                NetFTRDTPacket cRDTData; /*the latest f/t data from the Net F/T*/
                int field1 = 500000, field2 = 510000, field3 = 520000, field4 = 530000, field5 = 540000, field6 = 550000;
                while (m_bKeepGoing)
                {
                    try
                    {
                        /*synchronize this in case they press the stop button
                             *while we're in the middle of reading the data*/
                        lock (m_netFT)
                        {
                            if (m_bDoingHighSpeedCollection)
                            {
                                /*read batch RDT packets*/
                                int PACKETS_TO_READ = 1000;
                                NetFTRDTPacket[] caRDTPackets = new NetFTRDTPacket[PACKETS_TO_READ];
                                caRDTPackets = m_netFT.readHighSpeedData(m_cNetFTDataSocket, PACKETS_TO_READ);
                                int i; /*generic loop/array index*/
                                /*Precondition: caRDTPackets contains the list of
                                 *packets that were read from the Net F/T.
                                *m_cDataWriter is open to file to collect data to.
                                *Postcondition: m_cDataWriter has the new F/T data.
                                *i == PACKETS_TO_READ.
                                */
                                for (i = 0; i < PACKETS_TO_READ; i++)
                                {
                                    Console.WriteLine(dataCollectionLine(caRDTPackets[i]));
                                    //m_cDataWriter.println( dataCollectionLine( caRDTPackets[i] ) );
                                }
                                cRDTData = caRDTPackets[PACKETS_TO_READ - 1];
                            }
                            else
                            {
                                cRDTData = m_netFT.readSingleFTRecord();
                            }
                        }
                        // deduct biasing values from rdt packet
                        m_guiParent.UpdateRobot(cRDTData);
                        m_guiParent.displayFTData(cRDTData, NUM_FT_AXES);
                    }
                    catch (SocketException sexc)
                    {
                        m_guiParent.displayError("Socket Exception: " + sexc.Message);
                    }
                    catch (IOException iexc)
                    {
                        m_guiParent.displayError("IO Exception: " +
                                iexc.Message);
                    }
                    try
                    {
                        if (!m_bDoingHighSpeedCollection)
                        {
                            Thread.Sleep(1000/refreshrate);
                        }
                    }
                    catch (Exception)
                    {
                        /*do nothing, just continue.  This exception should
                         only be thrown if they try to stop the thread while
                         it's sleeping*/
                    }
                }
            }
        }

        // Constructor
        public FTsensor(string ipAddress, MainForm set_parent, int sensor_id)
        {
            m_strSensorAddress = ipAddress;
            parent = set_parent;
            this.sensor_id = sensor_id;
        }

        /**Formats a data packet for collection to file.
         * @param nftrdtp   The data packet to record to file.
         * @return  A formatted string containing the F/T data and current time,
         * which can be written to file.
         */

        private static String dataCollectionLine(NetFTRDTPacket nftrdtp)
        {
            String outputString =  /* The formatted output line. */
                    "0x" + Convert.ToString(nftrdtp.getStatus(), 16) + "," +
                    nftrdtp.getRDTSequence() + "," +
                    nftrdtp.getFTSequence() + "," +
                    nftrdtp.getFx() + "," +
                    nftrdtp.getFy() + "," +
                    nftrdtp.getFz() + "," +
                    nftrdtp.getTx() + "," +
                    nftrdtp.getTy() + "," +
                    nftrdtp.getTz() + "," +
                    DateTime.Now.Ticks;
            return outputString;
        }

        /**
         * Stops the F/T reader thread, and sets m_NetFTReaderThread =
         * null.
         */

        private void stopReaderThread()
        {
            if (null != m_NetFTReaderThread)
            {
                m_NetFTReaderThread.stopReading();
                m_NetFTReaderThread = null;
            }
        }

        /**Stops (if necessary) the net f/t reader thread, and restarts it using
         *the user options. Also sets the scaling factors for the f/t display.
         */

        public void stopAndRestartSingleReadings()
        {
            if (null != m_NetFTReaderThread)
            {
                m_NetFTReaderThread.stopReading();
                m_NetFTReaderThread = null;
            }
            try
            {
                m_netFT = new NetFTSensor(System.Net.IPAddress.Parse(m_strSensorAddress));
            }

            catch (Exception uhex)
            {
                displayError("Unknown Host Exception: " + uhex.Message);
                return;
            }
            m_NetFTReaderThread = new NetFTReaderThread(m_netFT, this);
            m_NetFTReaderThread.Start();
        }

        /**
         *Sets the maximum ratings for each axis.
         *@param strConfigPageText  The HTML code of the configuration page on the
         *integrated web server.
         */

        private void setCountsPerForce(double counts)
        {
            double dCountsPerForce = counts;
            if (0 == dCountsPerForce)
            {
                dCountsPerForce = 1;
                displayError("Read a counts per force value of 0, setting it to " + "1 instead.");
            }
            int i;
            for (i = 0; i < 3; i++)
            {
                m_daftCountsPerUnit[i] = dCountsPerForce;
            }
        }

        private void setCountsPerTorque(double counts)
        {
            double dCountsPerTorque = counts;
            if (0 == dCountsPerTorque)
            {
                dCountsPerTorque = 1;
                displayError("Read a counts per torque value of 0, setting it " +
                        "to 1 instead.");
            }
            int i; /*generic loop/array index.*/
            /*Precondition: dCountsPerForce has the counts per force,
             *dCountsPerTorque is the counts per torque.
             *Postcondition: m_daftCountsPerUnit has the counts per unit for each
             *axis, i == 3.
             */
            for (i = 0; i < 3; i++)
            {
                m_daftCountsPerUnit[i + 3] = dCountsPerTorque;
            }
        }

        /**Display an error in the last error field, including the time that
         *the error occurred.  Does the displaying using InvokeLater, so it's
         *thread safe.
         *@param errDesc    The String to display in the last error field.  The
         *time will be displayed in front of this text, so there is no need to put
         *the time in yourself.
         */

        public void displayError(String errDesc)
        {
            Console.WriteLine("error: " + errDesc);
        }

        /**This method gets the latest force and torque values, and displays
         *them on the screen.
         */

        private void GetFTValues()
        {
            NetFTRDTPacket rdtData;
            try
            {
                rdtData = m_netFT.readSingleFTRecord();
            }
            catch (SocketException sexc)
            {
                displayError("SocketException: " + sexc.Message);
                return;
            }
            catch (IOException iexc)
            {
                displayError("IOException: " + iexc.Message);
                return;
            }
        }

        // relay display command to parent
        private void displayFTData(NetFTRDTPacket displayRDT, int NUM_FT_AXES)
        {
            parent.updateFTData(displayRDT, NUM_FT_AXES, m_daftCountsPerUnit, this.sensor_id);
        }

        // Control system stuff
        public void UpdateRobot(NetFTRDTPacket RDT)
        {
            parent.UpdateRobot(RDT, this.sensor_id);
        }

        internal bool is_sending()
        {
            return parent.IsSending();
        }
    }
}